﻿#region License
/**	
The MIT License (MIT)

Copyright (c) 2013 Philipp Schillinger, Michael Alekseew, Andreas Braun, Steeven Zeiß

Permission is hereby granted, free of charge, to any person obtaining a copy
of this software and associated documentation files (the "Software"), to deal
in the Software without restriction, including without limitation the rights
to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
copies of the Software, and to permit persons to whom the Software is
furnished to do so, subject to the following conditions:

The above copyright notice and this permission notice shall be included in
all copies or substantial portions of the Software.

THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
THE SOFTWARE. 
 **/
#endregion

using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Emgu.CV;
using Emgu.CV.Structure;

namespace IndoorLocalization
{
    /// <summary>
    /// If a sensor detects something, it creates one of this SensorRays.
    /// Saves data about the detection and can be used to evaluate it.
    /// </summary>
    public class SensorRay {

		private HCoord origin;
		/// <summary>
		/// Origin of this ray.
		/// </summary>
		public HCoord Origin { get { return origin; } }

		private HCoord line;
		/// <summary>
		/// Line representation of this ray to be used for calculation.
		/// </summary>
		public HCoord Line { get { return line; } }

        /// <summary>
        /// The x position of the origin of this ray.
        /// </summary>
        public int X { get { return origin.X; } }

        /// <summary>
        /// The y position of the origin of this ray.
        /// </summary>
        public int Y { get { return origin.Y; } }

        private float angle;
        /// <summary>
        /// The angle of this ray in degrees.
		/// Zero degrees points in positive x direction.
        /// </summary>
        public float Angle { get { return angle; } }

		private int estimatedDistanceIndicator;
		/// <summary>
		/// An indicator for the estimated distance to the detected object.
		/// A value between 0 and 10, where 10 means far.
		/// </summary>
		public int EstimatedDistanceIndicator { 
			get { return estimatedDistanceIndicator; }
			set { estimatedDistanceIndicator = value; } 
		}

        private SensorCam creatorCam;
        /// <summary>
        /// The sensor which created this ray.
        /// </summary>
		public SensorCam CreatorCam { get { return creatorCam; } }

		private Guid creator;
		/// <summary>
		/// The ID of the sensor which created this ray.
		/// </summary>
		public Guid Creator { get { return creator; } }

        private Bgr avgColor;
        /// <summary>
        /// The average color of the area hit by the ray.
        /// </summary>
        public Bgr AvgColor { 
            get { return avgColor; } 
            set { avgColor = value; }
        }

        private DenseHistogram blueHist;
        /// <summary>
        /// The blue color histogram of the area hit by the ray.
        /// </summary>
        public DenseHistogram BlueHist
        {
            get { return blueHist; }
            set { blueHist = value; }
        }

        /// <summary>
        /// Should be used to create a new ray if something was detected.
        /// </summary>
        /// <param name="direction">The direction in which the sensor detected something.</param>
        /// <param name="sensor">The sensor which detected something.</param>
        public SensorRay(float direction, SensorCam sensor) {
			this.origin = new HCoord(sensor.X, sensor.Y);
            this.angle = (direction + sensor.Orientation) % 360;
			this.line = new HCoord(sensor.X, sensor.Y, angle);
            this.creatorCam = sensor;
			this.creator = sensor.GUID;
        }

		/// <summary>
		/// Creates a new ray out of origin and direction data.
		/// </summary>
		/// <param name="origin">The origin of the ray to create.</param>
		/// <param name="direction">The direction of the ray to create.</param>
		/// <param name="creatorID">The ID of the creator.</param>
		public SensorRay(HCoord origin, float direction, Guid creatorID) {
			this.origin = origin;
			this.angle = direction;
			this.line = new HCoord(origin.X, origin.Y, angle);
			this.creator = creatorID;
			this.creatorCam = null;
		}

        /// <summary>
		/// Only use this method if the intersect point is really not needed.
		/// Else use <c>IntersectPoint(SensorRay)</c> instead
		/// to preserve the intersect point which is calculated implicitly.
		/// 
		/// Determines if this ray intersects the other one.
		/// Returns <c>false</c> in each of this cases:
		/// - they do not intersect
		/// - they intersect out of bounds
		/// - they intersect in a wall
		/// - a wall is between the intersect point and at least one of the origins
		/// - the intersect point is "behind" of at least one origin (angle rotated 180 degrees)
        /// </summary>
        /// <param name="other">Rays that should be tested.</param>
        /// <returns><c>true</c> if they intersect, <c>false</c> otherwise.</returns>
        public bool DoesIntersect(SensorRay other) {
            return IntersectPoint(other) != null;
        }

		/// <summary>
		/// Calculates the intersect point of this ray and the given one.
		/// Returns <c>null</c> in each of this cases:
		/// - they do not intersect
		/// - they intersect out of bounds
		/// - they intersect in a wall
		/// - a wall is between the intersect point and at least one of the origins
		/// - the intersect point is "behind" of at least one origin (angle rotated 180 degrees)
		/// </summary>
		/// <param name="other">Ray that probably intersects this one.</param>
		/// <returns>The intersect point or <c>null</c>.</returns>
		public HCoord IntersectPoint(SensorRay other) {
			// calculate intersect point (they always intersect, in parallel case far out of bounds)
			HCoord ip = HCoord.Cross(line, other.Line);

			// check if ip is "behind" one of the origins
			if (   (ip.X - this.X) * Math.Cos(this.Angle / 180 * Math.PI) < 0
				|| (ip.Y - this.Y) * Math.Sin(this.Angle / 180 * Math.PI) < 0
				|| (ip.X - other.X) * Math.Cos(other.Angle / 180 * Math.PI) < 0
				|| (ip.Y - other.Y) * Math.Sin(other.Angle / 180 * Math.PI) < 0)
				return null;
            // check if at the origin
            if (ip.DistanceTo(this.origin) < 2 ||
                ip.DistanceTo(other.origin) < 2)
                return null;
			// check if ip is in bounds an not in a wall
			if (!FloorPlan.IsValidPosition(ip))
				return null;
			// check if there is a wall in between
			if (   FloorPlan.IsWallBetween(ip, this.origin)
				|| FloorPlan.IsWallBetween(ip, other.origin))
				return null;

			// intersect point is valid
			return ip;
		}

        /// <summary>
        /// Calculates the distance between a supplied histogram and the stored blue histogram of a ray.        
        /// </summary>
        /// <param name="compare">Histogram of a blue channel.</param>
        /// <returns>The distance using correlation histogram compare.</returns>
        public double histoDistance(DenseHistogram compare) {
            return CvInvoke.cvCompareHist(blueHist.Ptr, compare.Ptr, Emgu.CV.CvEnum.HISTOGRAM_COMP_METHOD.CV_COMP_CORREL);
        }


    }
}
